#include "KMotionDef.h"
#include "MySpindleDefs.h"


//  R I G I D   T A P P I N G 

#define ZAXIS 2			//Axis to slave
#define SPINDLE_AXIS 7		//Spindle Axis With Encoder Feedback (Different From SPINDLEAXIS)
#define Z_CPI 50800		//Counts per inch of slave axis
#define CPR 4096		//Counts per rev of spindle axis
#define TAU 0.010		//Smoothing time
#define MAXTAPSPEED 2000	//Maximum Tapping speed in rpm
#define APPROACHRPM 100         //Speed at which the spindle creeps up to target tap depth

double SlaveGain,ToCut,Total,Z0,S0,ZDir,OSDecel,OSStop;
void Slave(void);

main()
{
	
        float Speed	= *(float *)&persist.UserData[KMVAR];                   //Current spindle speed
	float DPR		= *(float *)&persist.UserData[0];      		//(MCode Parameter P) Distance per rev (Thread Pitch)
        float Depth 	= *(float *)&persist.UserData[1];                       //(MCode Parameter Q) Depth in incremental coords from current position
	float SDir		= *(float *)&persist.UserData[2];      		//(MCode Parameter R) Thread Direction (1=RH, -1=LH)
	
	float appRPM = APPROACHRPM;
	float decelSpeed = appRPM / MAXRPM;
	//printf("DPR=%f\n",DPR);
	//printf("Depth=%f\n",Depth);
	//printf("Dir=%f\n",SDir);

	// Check to see if commanded speed is less than maximum allowable speed for tapping

	if (Speed > MAXTAPSPEED){
		Speed = MAXTAPSPEED;
	}

	// Check if valid R word (rotational direction)

	if (SDir != 1 && SDir != -1)
	{
		printf("Invalid R Word\n");
		printf("Exiting Tapping Function\n");
		return;
	}

	// Stop Spindle

	Jog(SPINDLEAXIS, 0.0);
	ClearBit(SPINDLERUN_BIT);
	persist.UserData[STATEVAR] = 0;  						// remember spindle is stopped
        while (!(CheckDone(SPINDLEAXIS) && ReadBit(SPINDLEZEROSPEED_BIT)))
	{
		WaitNextTimeSlice();
	}

	// Prep Tapping Parameters
	
	//OSStop = 0;
	//OSDecel = 0;
	OSStop = Z_CPI * DPR*(0.00000135114*APPROACHRPM*APPROACHRPM + 0.0027943*APPROACHRPM);		// Overshoot Stop
	OSDecel = Z_CPI * DPR*(0.00000135114*Speed*Speed + 0.0027943*Speed + 0.75);		// Overshoot Decel
	ZDir = 0; 									// Initial Z Axis Movement Direction ( 0 = Down )	
	int Done = 0;
	int Slowed = 0;
	SlaveGain = Z_CPI * DPR * (-SDir) / CPR;
	Z0 = chan[ZAXIS].Dest;
	float ToCut = Depth * Z_CPI;                   					//Calculted incremental distance
	float DecelPoint = Z0 - ToCut + OSDecel;          					//Position where spindle decelerates to 100 RPM (Absolute Position)
	float StopPoint = Z0 - ToCut +  OSStop;					//Position where spindle is command to stop (Absolute Position)
	float Bottom = Z0 - ToCut;
	Zero(SPINDLE_AXIS);
	S0 = chan[SPINDLE_AXIS].Position;
	//printf("Decel Point=%f\n",DecelPoint);
	//printf("Stop Point=%f\n",StopPoint);
	//printf("Bottom=%f\n",Bottom);

	// Start Spindle
	
	WaitNextTimeSlice();
	SetBit(SPINDLERUN_BIT);
	persist.UserData[STATEVAR] = SDir;  						// Remember spindle direction
	if (SDir == 1)
	{
		SetBit(SPINDLECW_BIT);
	} else {
		SetBit(SPINDLECCW_BIT);
	}
	Jog(SPINDLEAXIS, Speed / MAXRPM);

	// Begin Tapping Operation
	
        while(Done == 0)        //When Done is equal to 1 then tapping is done
	{
		Slave();
                if ((chan[ZAXIS].Dest <= DecelPoint) && (ZDir == 0) && (Slowed == 0))           // Decel Point Reached. Ramp Spindle Speed to Decel Speed
		{
			Jog(SPINDLEAXIS, decelSpeed);
			Slowed = 1;
		}
		if ((chan[ZAXIS].Dest <= StopPoint) && (ZDir == 0))				// Bottom Reached
		{

			if (SDir == 1)
			{
				ClearBit(SPINDLECW_BIT);
				Jog(SPINDLEAXIS, 0.0);
                                while (!(CheckDone(SPINDLEAXIS) && ReadBit(SPINDLEZEROSPEED_BIT)))      // Wait until SPINDLEAXIS is done moving and Spindle Zero Speed Relay goes high
				{
					Slave();					// Continue moving Z-Axis while spindle slows to a stop
				}
				
                                //int delay = 0;                                        // Uncomment to implement a delay at the bottom of the tap to allow for overshoot reading
                                //while(delay < 2000){                                  // This creates a delay however the Z Axis continues to move so that overshoot can be seen
                                //	Slave();                                        // Spindle is still spinning because SPINDLEAXIS is an open loop and reaches 0 before the
                                //	delay++;                                        // the spindle speed reaches 0. SPINDLEZEROSPEED goes high below a threshold RPM and not at
                                //	WaitNextTimeSlice();                            // actual 0 rpm.
				//}	
                                SetBit(SPINDLECCW_BIT);                                 //Reverse Direction
				Jog(SPINDLEAXIS, Speed / MAXRPM);
				persist.UserData[STATEVAR] = -1;  			// remember spindle is set to CCW

			} else if (SDir == -1){
				
				ClearBit(SPINDLECCW_BIT);
				Jog(SPINDLEAXIS, 0.0);
                                while (!(CheckDone(SPINDLEAXIS) && ReadBit(SPINDLEZEROSPEED_BIT)))
				{
					Slave();					// Continue moving Z-Axis while spindle slows to a stop
				}	
                                SetBit(SPINDLECW_BIT);                                  //Reverse Direction
				Jog(SPINDLEAXIS, Speed / MAXRPM);
				persist.UserData[STATEVAR] = 1;  			// remember spindle is set to CW
			
			}

			ZDir = 1;
			
		}

		if ((chan[ZAXIS].Dest >= Z0) && (ZDir == 1))				// Tap Complete
		{
		
			Done = 1; 
			Jog(SPINDLEAXIS, 0.0);
			ClearBit(SPINDLECW_BIT);
			ClearBit(SPINDLECCW_BIT);
			ClearBit(SPINDLERUN_BIT);
			persist.UserData[STATEVAR] = 0;  				// remember spindle is stopped

		}	

	}

	Move(ZAXIS, Z0); 								// Move ZAxis to initial position
	while (!CheckDone(ZAXIS));
	printf("Tap Function Complete\n");

	    
}

void Slave(void)
{
	float Destination = (chan[SPINDLE_AXIS].Position-S0) * SlaveGain + Z0;
	MoveExp(ZAXIS, Destination, TAU);
	WaitNextTimeSlice();
}